#ifndef __QUATERNION_H__
#define __QUATERNION_H__

#define USE_NLOHMANN_JSON 1
#define USE_FMT_TO_STRING 1

#include "Matrix.h"
#include "Vector3.hpp"
#include "MathUtils.hpp"

#if USE_NLOHMANN_JSON
#include "nlohmann/json.hpp"
#endif
#if USE_FMT_TO_STRING
#include "spdlog/fmt/fmt.h"
#endif


namespace PocketSurvival::Engine
{
    struct Quaternion
    {
        float X;
        float Y;
        float Z;
        float W;

        constexpr Quaternion() noexcept;
        constexpr Quaternion(float x, float y, float z, float w) noexcept;

        constexpr inline Quaternion &operator*=(const Quaternion& right);

        inline float length() const;
        constexpr inline float lengthSquared() const;
        Matrix toMatrix() const;
        Vector3 toYawPitchRoll() const;
		constexpr inline Vector3 GetUpVector() noexcept;
		constexpr inline Vector3 GetForwardVector() noexcept;
        
        static inline Quaternion Conjugate(Quaternion &q)
        {
            return Quaternion { -q.X, -q.Y, -q.Z, q.W };
        }
        static inline void IndetityInit(Quaternion &qua) noexcept
        {
            qua.X = 0.0f;
            qua.Y = 0.0f;
            qua.Z = 0.0f;
            qua.W = 1.0f;
        }
        static Quaternion Normalize(const Quaternion &q)
        {
			float num = q.length();
			if (num == 0.0f)
			{
				return Quaternion { 0.0f, 0.0f, 0.0f, 1.0f };
			}
			return q / num;
        }
        static void CreateFromAxisAngle(const Vector3 &axis, float angle, Quaternion& result);
        static void CreateFromRotationMatrix(Matrix &m, Quaternion &result);
        static void CreateFromYawPitchRoll(float yaw, float pitch, float roll, Quaternion &result);
        static void Lerp(const Quaternion &q1, const Quaternion &q2, float f, Quaternion &result);
        static void Slerp(const Quaternion &q1, const Quaternion &q2, float f, Quaternion &result);
        static Vector3 Vector3Transform(const Vector3 &v, const Quaternion &q);
        
        friend constexpr inline Quaternion operator/(const Quaternion &q, float d)
        {
			float num = 1.0f / d;
			return Quaternion { q.X * num, q.Y * num, q.Z * num, q.W * num };
        }
        friend constexpr inline Quaternion operator*(const Quaternion &left, const Quaternion &right)
        {
			float x = left.X;
			float y = left.Y;
			float z = left.Z;
			float w = left.W;
			float x2 = right.X;
			float y2 = right.Y;
			float z2 = right.Z;
			float w2 = right.W;
			float num = y * z2 - z * y2;
			float num2 = z * x2 - x * z2;
			float num3 = x * y2 - y * x2;
			float num4 = x * x2 + y * y2 + z * z2;
            return Quaternion {
			    x * w2 + x2 * w + num,
			    y * w2 + y2 * w + num2,
			    z * w2 + z2 * w + num3,
			    w * w2 - num4
            };
        }
        friend constexpr inline bool operator==(const Quaternion &left, const Quaternion &right)
        {
            return (left.X == right.X) && (left.Y == right.Y) && (left.Z == right.Z) && (left.W == right.W);
        }
        friend constexpr inline bool operator!=(const Quaternion &left, const Quaternion &right)
        {
            return !(left == right);
        }

#if USE_NLOHMANN_JSON
        friend inline void to_json(nlohmann::json &json, const Quaternion &vec);
        friend inline void from_json(const nlohmann::json &json, Quaternion &vec);
#endif

#if USE_FMT_TO_STRING
        inline std::string toString()
        {
            return fmt::format("{},{},{},{}",X,Y,Z,W);
        }
#endif
    };
    
    constexpr inline Quaternion::Quaternion() noexcept : X(0), Y(0), Z(0), W(0) {}
    constexpr inline Quaternion::Quaternion(float x, float y, float z, float w) noexcept : X(x), Y(y), Z(z), W(w) {}

    constexpr inline Quaternion &Quaternion::operator*=(const Quaternion& right)
    {
        float x = X;
        float y = Y;
        float z = Z;
        float w = W;
        float x2 = right.X;
        float y2 = right.Y;
        float z2 = right.Z;
        float w2 = right.W;
        float num = y * z2 - z * y2;
        float num2 = z * x2 - x * z2;
        float num3 = x * y2 - y * x2;
        float num4 = x * x2 + y * y2 + z * z2;
        X = x * w2 + x2 * w + num;
        Y = y * w2 + y2 * w + num2;
        Z = z * w2 + z2 * w + num3;
        W = w * w2 - num4;
        return *this;
    }

    inline float Quaternion::length() const
    {
        return MathUtils::Sqrt<float>(lengthSquared());
    }
    constexpr inline float Quaternion::lengthSquared() const
    {
        return X * X + Y * Y + Z * Z + W * W;
    }
	constexpr inline Vector3 Quaternion::GetUpVector() noexcept
	{
		return Vector3 { 2.0f * (X * Y - Z * W), 1.0f - 2.0f * (X * X + Z * Z), 2.0f * (Y * Z + X * W) };
	}
	constexpr inline Vector3 Quaternion::GetForwardVector() noexcept
	{
		return Vector3 { -2.0f * (Y * W + X * Z), 2.0f * (X * W - Y * Z), 2.0f * (X * X + Y * Y) - 1.0f };
	}


#if USE_NLOHMANN_JSON

    inline void to_json(nlohmann::json &json, const Quaternion &vec)
    {
        float xValue = std::isnan(vec.X) ? 0 : vec.X;
        float yValue = std::isnan(vec.Y) ? 0 : vec.Y;
        float zValue = std::isnan(vec.Z) ? 0 : vec.Z;
        float wValue = std::isnan(vec.W) ? 0 : vec.W;

        json = {{"X",xValue},{"Y",yValue},{"Z",zValue},{"W", wValue}};
    }
    inline void from_json(const nlohmann::json &json, Quaternion &vec)
    {
        vec.X = json["X"].get<float>();
        vec.Y = json["Y"].get<float>();
        vec.Z = json["Z"].get<float>();
        vec.W = json["W"].get<float>();
    }

#endif


} // namespace PocketSurvial

#undef USE_NLOHMANN_JSON
#undef USE_FMT_TO_STRING

#endif
